package LBC;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.Stack;

import org.newdawn.slick.Color;

import Objects.Body;
import Objects.BodyCircle;
import Objects.BodyRectangle;

public class CollisionListener extends Thread{

        @SuppressWarnings("unchecked")
        public HashMap []hM;
        public HashMap<Integer, Body> bodies;
    public HashMap<Body, BodyRectangle> btr;
    public HashMap<Body, BodyCircle> btc;
        Stack<Body> bs;
        Body Body1;
        Body Body2;
        Color white;
        Color acol;
        float impactX, impactY;
        Integer time;
        Contact contact;
        ArrayList <Contact> contacts;
        ContactThinggy ct;
        float relativeNormalVelocity, relativeNormalVelocityX, relativeNormalVelocityY;
        float vA, vB;
        float []normal;
        @SuppressWarnings("unchecked")
        public CollisionListener(HashMap[] hM, Integer time){
            this.hM=hM;
            bodies=hM[0];
            btr=hM[1];
            btc=hM[2];
                bs=new Stack<Body>();
                white=new Color(255,255,255);
                acol=new Color(255,100,100);
                this.time=time;
        }


        public void bodyChecker2(){
                bs.clear();
                for(int i=bodies.size()-1; i>=0; i--){
                        bs.push(bodies.get(i));
                }
                for(int i=0; i<=bs.size(); i++){
                        Body1=bs.pop();
                        if(bs.size()!=0){
                                for(int j=bodies.size()-bs.size(); j<bodies.size(); j++){
                                        //if(interaction(cBody1,cbodies.get(j))){

                                        float contactX=0;
                                        float contactY=0;

                                        if(Body1.shape.intersects(bodies.get(j).shape))
                                        {
                                                Body2=bodies.get(j);
                                                if(btr.get(Body1)!=null && btr.get(Body2)!=null)
                                                {
                                                        float []points1=Body1.shape.getPoints();
                                                        int maxX=getMaxX(points1);
                                                        int maxY=getMaxY(points1);
                                                        int minX=getMinX(points1);
                                                        int minY=getMinY(points1);
                                                        for(int k=minY; k<=maxY; k++){
                                                                for(int l=minX; l<=maxX; l++){
                                                                                        if(Body1.shape.contains(l, k) &&  Body2.shape.contains(l, k)){
                                                                                                contactX=l;
                                                                                                contactY=k;
                                                                                                Body1.colx=l;
                                                                                                Body1.coly=k;
                                                                                                Body2.colx=l;
                                                                                                Body2.coly=k;
                                                                                                time=0;
                                                                                                l=(int) points1[6];
                                                                                                k=(int) points1[3];
                                                                                        }
                                                                }
                                                        }
                                                        normal=(btr.get(Body1)).findNormal(contactX, contactY);
                                                }
                                                else if(btc.get(Body1)!=null && btc.get(Body2)!=null)
                                                {
                                                	normal=(btc.get(Body2)).findNormal(contactX, contactY);
                                                }
                                                else if(btr.get(Body1)!=null && btc.get(Body2)!=null)
                                                {
                                                	normal=(btr.get(Body1)).findNormal(contactX, contactY);
                                                }
                                                else if(btc.get(Body1)!=null && btr.get(Body2)!=null)
                                                {
                                                	normal=(btr.get(Body2)).findNormal(contactX, contactY);
                                                }
                                               
                                                
                                                System.out
														.println("weszlo?");
                                                //TODO check który mial byc chyba co do corrnerów i scian, ale nie mam pewnosci co do parametrow
//                                              if(btr.get(Body1)!=null){
//                                                      btr.get(Body1).check(p1, p2, contactX, contactY);
//                                              }
//                                              else
//                                              {
//                                                      if(btr.get(bodies.get(j))!=null){
//                                              }
//                                                      btr.get(bodies.get(j)).check(p1, p2, contactX, contactY);
//                                              }
                                               
//                                              vA=(float) Math.sqrt(Math.pow(Body1.getVx(),2)+Math.pow(Body1.getVy(), 2));
//                                              vB=(float) Math.sqrt(Math.pow(bodies.get(i).getVx(),2)+Math.pow(bodies.get(i).getVy(), 2));
//                                              relativeNormalVelocity=(vA-vB)*normal[];
//                                              relativeNormalVelocityX=(Body1.getVx()-bodies.get(i).getVx())*normalX;
//                                              relativeNormalVelocityY=(Body1.getVy()-bodies.get(i).getVy())*normalY;
//                                              relativeNormalVelocity=(float) (Math.sqrt((Math.pow(relativeNormalVelocityX,2))+(Math.pow(relativeNormalVelocityY,2))));;

                                                        if((Body1.velocityX+bodies.get(j).velocityX)!=0 || (Body1.velocityY+bodies.get(j).velocityY)!=0){
                                                                collision(Body1, bodies.get(j), normal, contactX, contactY);
                                                        }
                                                        else{
                                                                contact=new Contact(Body1, bodies.get(j), contactX, contactY, normal); //TODO function to decide which body should be used to determine normal
                                                                contact.a=Body1;
                                                                contact.b=bodies.get(j);
                                                                contacts.add(contact);
                                                        }
                                        }
                                        else
                                        {
                                                Body1.color=white;
                                                bodies.get(j).color=white;                                              
                                        }
                                }
                        }
                }
                if(contacts!=null)
                {
                ct.computeContactForces(contacts);
                }
        }
       
        private int getMinY(float[] points) {
                int result=(int) points[1];
                for(int i=3; i<points.length; i=i+2){
                        if(result>points[i]){
                                result=(int) points[i];
                        }
                }
                return result;
        }


        private int getMinX(float[] points) {
                int result=(int) points[0];
                for(int i=2; i<points.length; i=i+2){
                        if(result>points[i]){
                                result=(int) points[i];
                        }
                }
                return result;
        }


        private int getMaxY(float[] points) {
                int result=(int) points[1];
                for(int i=3; i<points.length; i=i+2){
                        if(result<points[i]){
                                result=(int) points[i];
                        }
                }
                return result;
        }


        private int getMaxX(float[] points) {
                int result=(int) points[0];
                for(int i=2; i<points.length; i=i+2){
                        if(result<points[i]){
                                result=(int) points[i];
                        }
                }
                return result;
        }


        public float [] GJK(BodyRectangle one , BodyRectangle two){


                return null;

        }






        public void ifcollison(Body A, Body B){

                A.forceX+=B.forceX;
                B.forceX+=A.forceX;
                A.accelerationX=A.forceX/A.mass;
                A.accelerationY=A.forceY/A.mass;
                B.accelerationX=B.forceX/A.mass;
                B.accelerationY=B.forceY/A.mass;


                float r1x=(float)(Math.abs(A.CMX-impactX)); //CMX- centre of mass, impactX- punkt kolizji
                float r1y=(float)(Math.abs(A.CMY-impactY));
                float r2x=(float)(Math.abs(B.CMX-impactX));
                float r2y=(float)(Math.abs(B.CMY-impactY));

                A.epsilon+=(r1x*A.forceY - r1y*A.forceX);
                B.epsilon+=(r2x*B.forceY - r2y*B.forceX);
        }

        public void collision(Body a, Body b, float[] normal, float contactX, float contactY){
                int X=0;
                int Y=1;
               
                System.out.println("ARGH"+normal[X]+"   "+normal[Y]);
                if(normal[X]<0) normal[X]=-1; else normal[X]=1;
                if(normal[Y]<0) normal[Y]=-1; else normal[Y]=1;

                float relativeVelocityX=a.velocityX-b.velocityX;
                float relativeVelocityY=a.velocityY-b.velocityY;
                float relativeCOR=(a.COR*b.COR);

                //Equations 6 and 9 have the same numerators, so we'll save their value
                float numeratorX=(-(1+relativeCOR)*relativeVelocityX*normal[X]);
                float numeratorY=(-(1+relativeCOR)*relativeVelocityY*normal[Y]);
                //Denominator from Eq. 6 is a part of Eq. 9 denominator
                float denominatorX=(normal[X]*normal[X]*(1/a.mass+1/b.mass));
                float denominatorY=(normal[Y]*normal[Y]*(1/a.mass+1/b.mass));

                float impulseX=(numeratorX/denominatorX);
                float impulseY=(numeratorY/denominatorY);

                a.velocityX+=((impulseX/a.mass)*normal[X]);
                a.velocityY+=((impulseY/a.mass)*normal[Y]);
                System.out.println((impulseY/a.mass)*normal[Y]);

                b.velocityX-=((impulseX/b.mass)*normal[X]);
                b.velocityY-=((impulseY/b.mass)*normal[Y]);    

                ///SPIN///


                //find the distance between the impact point and the CM
                float rAX=a.shape.getCenterX()-contactX;
                float rAY=a.shape.getCenterY()-contactY;
                float rBX=b.shape.getCenterX()-contactX;
                float rBY=b.shape.getCenterY()-contactY;

                //Eq. 9 "Collision Response" C.Hecker
                float rAnX=-rAX*normal[X];
                float rAnY=rAY*normal[Y];
                float rBnX=-rBX*normal[X];
                float rBnY=rBY*normal[Y];

                float angularImpulseX=(float)((numeratorX)/(denominatorX+(Math.pow(rAnX,2)/a.momentOfInertia)+(Math.pow(rBnX, 2)/b.momentOfInertia)));
                float angularImpulseY=(float)((numeratorY)/(denominatorY+(Math.pow(rAnY,2)/a.momentOfInertia)+(Math.pow(rBnY, 2)/b.momentOfInertia)));

                //Eq. 8b "Collision Response" C.Hecker
                float numeratorA=(float)(Math.sqrt(Math.pow((rAnX*angularImpulseX),2))+(Math.pow((rAnY*angularImpulseY),2)));
                float numeratorB=(float)(Math.sqrt(Math.pow((rBnX*angularImpulseX),2))+(Math.pow((rBnY*angularImpulseY),2)));

                a.angularVelocity+=numeratorA/a.momentOfInertia;
                b.angularVelocity-=numeratorB/b.momentOfInertia;

        }


}